//
// Created by kai-xu on 2023/2/21.
//

#include "preprocess/align_pose_matrix.h"

#include <iostream>

// #include "preprocess/pro_mat_utils.h" // cuda
bool gen_align_matrix(float *out, const int sd, std::vector<float> pcr,
                      std::vector<float> vs, const int32_t height,
                      const int32_t width, const Eigen::Matrix4f &pre_matrix,
                      const Eigen::Matrix4f &cur_matrix) {
  // sd = 4 pcr:[-51.2, -51.2, -5.0, 51.2, 51.2, 3.0] vs:[0.2, 0.2, 8]
  float x_stride = vs[0] * sd;    // 0.8
  float y_stride = vs[1] * sd;    // 0.8
  float x_offset = x_stride / 2;  // 0.4
  float y_offset = y_stride / 2;  // 0.4
  float x_start_idx = pcr[0] + x_offset;
  float y_start_idx = pcr[1] + y_offset;

  Eigen::Matrix4f pose_matrix = cur_matrix.inverse() * pre_matrix;
  try {
    for (int32_t i{0}; i < height; i++) {
      float y_idx = y_start_idx + y_stride * i;
      for (int32_t j{0}; j < width; j++) {
        float x_idx = x_start_idx + x_stride * j;
        float tnp1 = pose_matrix(0, 0) * x_idx + pose_matrix(0, 1) * y_idx +
                     pose_matrix(0, 3) - x_start_idx;
        float tnp2 = pose_matrix(1, 0) * x_idx + pose_matrix(1, 1) * y_idx +
                     pose_matrix(1, 3) - y_start_idx;
        out[0 * width * height + i * width + j] =
            tnp1 / ((width - 1) * x_offset) - 1;
        out[1 * width * height + i * width + j] =
            tnp2 / ((height - 1) * y_offset) - 1;
      }
    }
  } catch (std::exception &e) {
    std::cout << "out of bound!" << std::endl;
    return false;
  }
  return true;
}

bool gen_ring_ray_map(float *circle_map_out, float *ray_map_out,
                      std::vector<float> const &Cam_Intrins,
                      std::vector<float> const &Cam_Extrins, float resize_scale,
                      Rect const &rect) {

  return true;
}
